#ifndef SENSOR_MMRAD_H_
#define  SENSOR_MMRAD_H_

#include <ros/ros.h>
#include<ros/time.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>


class Sensor_mmRad
{
private:
     //毫米波雷达模式数据设定命令
     uint8_t Mode[6]={0xA6,0x03,0x00,0x00,0x00,0x6A};
    //毫米波雷达属性数据配置命令
    uint8_t Config[21]= {0xFA,0x09,0x00,0x00,0x00,0x80,
                                0x00,0x00,0x00,0x00,0x00,0x00,
                                0x00,0x00,0x00,0x00,0x00,0x00,
                                0x00,0x00,0xFB};
    //毫米波雷达参数获取命令
    uint8_t fdback[6]={0xA6,0x03,0x00,0x00,0x00,0x6A};

    //毫米波扫描数据
    uint8_t ScanBuff[9]= {0x41,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x42};
    std::vector<uint8_t> reciveData;

    int m_alarmDistance;
    uint16_t distance ;
    uint16_t mag;
    uint16_t angle;//如果有角度，就使用
    

public:
    serial::Serial ser;
    //ros::NodeHandle nh;
    //ros::Subscriber MachineStatus_sub;

public:
    Sensor_mmRad();
    ~Sensor_mmRad();

    //向雷达设置模式命令
    void set_CommandMode(int type, int Var); 
    //设置安全距离
    void set_AlarmDistance(int Dist);   
    void setScanData(std::vector<uint8_t> data);
    void mmRadScan_Start();
    bool Init_mmRadComPort(std::string name);
    uint16_t comb16(uint8_t low, uint8_t high);
    void get_mmRadComScanData();
    uint16_t get_ActualDistance();
    uint16_t get_ActualAmplitude();
    uint16_t get_ActualAngle();
    bool ObstacleScan();
    void CloseComPort();
    uint8_t DataOXROperate(uint8_t* pt,int num);
};

#endif